#ifndef RADAR_HPP
#define RADAR_HPP

#include <iostream>
#include <string>
#include <future>
#include <thread>
#include <vector>
#include <mutex>
#include <condition_variable>
#include <termios.h>
#include <unistd.h>
#include <signal.h>
#include <termios.h>
#include <functional>
#include <kalman.hpp>

/*
 * Unitree robot forward :
 * rigtht turn is +;
 * left turn is -;
 * 
*/

#pragma pack(push, 1)
struct radar_frame {
    uint32_t MessageHeader;
    uint16_t PacketLength;
    uint16_t SequenceID;
    uint16_t RequestCommand;
    uint16_t VersionID;
    uint32_t AnchorID;
    uint32_t TagID;
    uint32_t Distance;
    int16_t Azimuth;
    int16_t Elevation;
    uint16_t TagStatus;
    uint16_t BatchSn;
    uint32_t Reserved;
    uint8_t  XorByte;
};
#pragma pack(pop)

class radar :public KalmanFilterBase {
public:
    radar();
    ~radar();
    void Init();
    void KalmanInit();
    static radar *GetInstance() {
        static radar instance;
        return &instance;
    }

    void Start();
    void Stop();

    void SetDetected(bool detected);
    void buffer_to_radar_frame(std::vector<uint8_t>& buffer);

    const struct radar_frame *GetRadarFrame();
    bool GetDetected();

    float getDistance()const;
    float getAngle()const;
    void setFollowDistance(float distance);
    void setFollowAngle(float angle);
    void setAngleDeviation(int16_t angle_deviation) {
        angle_deviation_ = angle_deviation;
    }
    void setDistanceDeviation(int16_t distance_deviation) {
        distance_deviation_ = distance_deviation;
    }
    int16_t getAngleDeviation() const {
        return angle_deviation_;
    }
    int16_t getDistanceDeviation() const {
        return distance_deviation_;
    }

    float getFollowDistance() const ;

    float getFollowAngle() const ;

    std::mutex mutex_;
    std::condition_variable cv_;
    std::atomic<bool> follow_mode_{false};

    void OnReceiveData(std::function<void(std::vector<uint8_t>& buffer)> callback);
private:
    float follow_distance_;
    float follow_angle_;
    int16_t angle_deviation_;
    int16_t distance_deviation_;
    int fd;
    int radar_data_len;
    bool detected_;
    struct radar_frame radar_frame_;
    struct termios ops_;
    struct sigaction sig_;
    bool accumulated_;
    std::vector<uint8_t> accumulated_data_;
    std::vector<uint8_t> radar_data;
    std::function<void(std::vector<uint8_t>& buffer)> callback_;
    std::vector<std::future<void>> void_task_futures_;
    
    static const char *RADAR_DEVICE;
    void radar_handle();
    void PrintRadarFrame(const struct radar_frame &frame);
};

#endif // RADAR_HPP